/*
 * Copyright (c) 2012 Johan Fylling
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights 
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 
 * THE SOFTWARE.
 */

part of engine;


class SapInterval {
  DynamicBody body;
  int         xMinIndex;
  int         xMaxIndex;
  int         yMinIndex;
  int         yMaxIndex;
  
  SapInterval (DynamicBody this.body,
               [int this.xMinIndex=0,
               int this.xMaxIndex=0,
               int this.yMinIndex=0,
               int this.yMaxIndex=0]);
}

class SapEdge {
  SapInterval interval;
  bool        isMin;
  bool        isXAxis;
  
  SapEdge (SapInterval this.interval, bool this.isMin, bool this.isXAxis);
  
  num get value {
    if (this.interval == null || 
        this.interval.body == null ||
        this.interval.body.collidable == null) {
      return 0.0;
    }
    
    AABB aabb = new AABB();
    this.interval.body.collidable.getAABB(aabb, this.interval.body);
    if (this.isMin) {
      return (this.isXAxis ? aabb.min.x : aabb.min.y);
    } else {
      return (this.isXAxis ? aabb.max.x : aabb.max.y);
    }
  }
}

/**
 * Broad-phase Collision Detector based on the Sweep And Prune (SAP) algorithm.
 * See [Sweep-and-prune, by Perre Terdiman, 11-09-2007].
 */
class SapDetector implements BroadphaseDetector {
  final List<CollisionPair> _collisionPairs;
  final List<SapEdge>       _xAxis;
  final List<SapEdge>       _yAxis;
  
  SapDetector () : this._collisionPairs = new List<CollisionPair>(),
                   this._xAxis = new List<SapEdge>(),
                   this._yAxis = new List<SapEdge>();
  
  /** See [BroadphaseDetector.collisionPairs]. */
  Collection<CollisionPair> get collisionPairs => this._collisionPairs;
  
  /** See [BroadphaseDetector.addWorldObject]. */
  bool addDynamicBody (DynamicBody body) {
    Collidable collidable = body.collidable;
    if (collidable == null) {
      return false;
    }
    
    SapInterval interval = new SapInterval(body);
    
    SapEdge xMinEdge = new SapEdge(interval, true, true);
    this._xAxis.addLast(xMinEdge);
    
    SapEdge xMaxEdge = new SapEdge(interval, false, true);
    this._xAxis.addLast(xMaxEdge);
    
    SapEdge yMinEdge = new SapEdge(interval, true, false);
    this._yAxis.addLast(yMinEdge);
    
    SapEdge yMaxEdge = new SapEdge(interval, false, false);
    this._yAxis.addLast(yMaxEdge);
  }
  
  /** Sort list of edges. */
  void _sort (List<SapEdge> edges) {
    if (edges.length < 2) {
      return;
    }
    
    for (int i = 0; i < edges.length; i++) {
      SapEdge current = edges[i];
      num value = current.value;
      
      int j;
      for (j = i-1; (j >= 0) && (edges[j].value > value); j--) {
        int newIndex = j+1;
        edges[newIndex] = edges[j];
        
        if (edges[newIndex].isXAxis && edges[newIndex].isMin) {
          edges[newIndex].interval.xMinIndex = newIndex;
        } else if (edges[newIndex].isXAxis && !edges[newIndex].isMin) {
          edges[newIndex].interval.xMaxIndex = newIndex;
        } else if (!edges[newIndex].isXAxis && edges[newIndex].isMin) {
          edges[newIndex].interval.yMinIndex = newIndex;
        } else {
          edges[newIndex].interval.yMaxIndex = newIndex;
        }
      }
      
      edges[j+1] = current;
      
      if (edges[j+1].isXAxis && edges[j+1].isMin) {
        edges[j+1].interval.xMinIndex = j+1;
      } else if (edges[j+1].isXAxis && !edges[j+1].isMin) {
        edges[j+1].interval.xMaxIndex = j+1;
      } else if (!edges[j+1].isXAxis && edges[j+1].isMin) {
        edges[j+1].interval.yMinIndex = j+1;
      } else {
        edges[j+1].interval.yMaxIndex = j+1;
      }
    }
  }
  
  /** See [BroadphaseDetector.detectBodyCollisions]. */
  List<CollisionPair> detectBodyCollisions (DynamicBody body, [ConfirmCollision cc]) {
    _sort(this._xAxis);
    _sort(this._yAxis);
    
    List<SapInterval> activeIntervals = new List<SapInterval>();
    List<CollisionPair> collisionPairs = new List<CollisionPair>();
    AABB aabb = new AABB();
    body.collidable.getAABB(aabb, body);
    
    // Find all intervals intersecting the AABB on the x-axis.
    for (SapEdge edge in this._xAxis) {
      num edgeValue = edge.value;
      
      if (edgeValue > aabb.max.x) {
        // We're outside the bounds of the object's AABB, no point in continuing.
        break;
      }
      
      if (edge.interval.body.id == body.id) {
        // The object cannot collide with itself.
        continue;
      }
      
      if (edge.isMin) {
        activeIntervals.add(edge.interval);
      } else if (edgeValue < aabb.min.x) {
        for (int i = 0; i < activeIntervals.length; i++) {
          if (identical(edge.interval, activeIntervals[i])) {
            activeIntervals.removeRange(i, 1);
            break;
          }
        }
      }
    }
    
    // Prune all intervals not intersecting on the y-axis.
    for (SapInterval activeInterval in activeIntervals) {
      AABB aabb2 = new AABB();
      activeInterval.body.collidable.getAABB(aabb2, activeInterval.body);
      if (aabb2.min.y >= aabb.min.y && aabb2.min.y <= aabb.max.y ||
          aabb2.max.y >= aabb.min.y && aabb2.max.y <= aabb.max.y ||
          aabb.min.y >= aabb2.min.y && aabb.min.y <= aabb2.max.y ||
          aabb.max.y >= aabb2.min.y && aabb.max.y <= aabb2.max.y) {
        if (!(?cc) || cc(activeInterval.body, body)) {
          CollisionPair pair = new CollisionPair(activeInterval.body, body);
          collisionPairs.add(pair);
        }
      }
    }
    
    return collisionPairs;
  }
  
  /** See [BroadphaseDetector.detectCollisions]. */
  void detectCollisions ([ConfirmCollision cc]) {
    _sort(this._xAxis);
    _sort(this._yAxis);
    
    this._collisionPairs.clear();
    
    List<SapInterval> activeIntervals = new List<SapInterval>();
    
    for (SapEdge edge in this._xAxis) {
      if (edge.isMin) {
        // The start of a new interval. Check for interference with active intervals.
        for (SapInterval activeInterval in activeIntervals) {
          // An active interval is interfering with the current interval. Check if the intervals also interfere on the y-axis.
          if (activeInterval.yMinIndex >= edge.interval.yMinIndex && activeInterval.yMinIndex <= edge.interval.yMaxIndex ||
              activeInterval.yMaxIndex >= edge.interval.yMinIndex && activeInterval.yMaxIndex <= edge.interval.yMaxIndex ||
              edge.interval.yMinIndex >= activeInterval.yMinIndex && edge.interval.yMinIndex <= activeInterval.yMaxIndex ||
              edge.interval.yMaxIndex >= activeInterval.yMinIndex && edge.interval.yMaxIndex <= activeInterval.yMaxIndex) {
            if (!(?cc) || cc(activeInterval.body, edge.interval.body)) {
              CollisionPair pair = new CollisionPair(activeInterval.body, edge.interval.body);
              this._collisionPairs.add(pair);
            }
          }
        }
        activeIntervals.addLast(edge.interval);
      } else {
        // We have reached the end of the interval. Remove it from the list of active intervals.
        for (int i = 0; i < activeIntervals.length; i++) {
          if (identical(edge.interval, activeIntervals[i])) {
            activeIntervals.removeRange(i, 1);
            break;
          }
        }
      }
    }
  }
  
  /** See [BroadphaseDetector.cull]. */
  List<DynamicBody> cull (AABB aabb) {
    _sort(this._xAxis);
    _sort(this._yAxis);
    
    List<SapInterval> activeIntervals = new List<SapInterval>();
    List<DynamicBody> collisions = new List<DynamicBody>();
    
    // Find all intervals intersecting the AABB on the x-axis.
    for (SapEdge edge in this._xAxis) {
      num edgeValue = edge.value;
      
      if (edgeValue > aabb.max.x) {
        // We're outside the bounds of the object's AABB, no point in continuing.
        break;
      }
      
      if (edge.isMin) {
        activeIntervals.add(edge.interval);
      } else if (edgeValue < aabb.min.x) {
        for (int i = 0; i < activeIntervals.length; i++) {
          if (identical(edge.interval, activeIntervals[i])) {
            activeIntervals.removeRange(i, 1);
            break;
          }
        }
      }
    }
    
    // Prune all intervals not intersecting on the y-axis.
    for (SapInterval activeInterval in activeIntervals) {
      AABB aabb2 = new AABB();
      activeInterval.body.collidable.getAABB(aabb2, activeInterval.body);
      if (aabb2.min.y >= aabb.min.y && aabb2.min.y <= aabb.max.y ||
          aabb2.max.y >= aabb.min.y && aabb2.max.y <= aabb.max.y ||
          aabb.min.y >= aabb2.min.y && aabb.min.y <= aabb2.max.y ||
          aabb.max.y >= aabb2.min.y && aabb.max.y <= aabb2.max.y) {
        collisions.add(activeInterval.body);
      }
    }
    
    return collisions;
  }
}
